#include "Robot.h"

Robot::Robot() :

	driveFLeft(3),
	driveFRight(4),
	driveBLeft(5),
	driveBRight(6),
	liftMotor(2),
	myRobot(driveFLeft, driveBLeft, driveFRight, driveBRight),
	rightStick(0),
	leftStick(1),
	xbox(2),
	launch(3),
	comp(1),
	shift(1,4),
	shiftRetract(1,5),
	leftArm(1,0),
	leftArmRetract(1,1),
	rightArm(1,2),
	rightArmRetract(1,3),
	liftSensor(0),
	autonMode1(7),
	autonMode2(8),
	liftEncoder(1, 2, false, Encoder::EncodingType::k4X),
	leftDrive(3, 4, false, Encoder::EncodingType::k4X),
	rightDrive(5, 6, false, Encoder::EncodingType::k4X),
	gyro(0)

{
	myRobot.SetExpiration(0.1);
}

void Robot::RobotInit() {


	myRobot.SetSafetyEnabled(false);
	gyro.InitGyro();
	rightDrive.SetReverseDirection(true);

	rightDrive.SetDistancePerPulse((15 * 3.14/256)/4);
	leftDrive.SetDistancePerPulse((15 * 3.14/256)/4);
	liftEncoder.SetDistancePerPulse((3.75/440));

	// create an image
	frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
	//the camera name (ex "cam0") can be found through the roborio web interface
	imaqError = IMAQdxOpenCamera("cam1", IMAQdxCameraControlModeController, &session);
	if(imaqError != IMAQdxErrorSuccess) {
		DriverStation::ReportError("IMAQdxOpenCamera error: " + std::to_string((long)imaqError) + "\n");
	}
	imaqError = IMAQdxConfigureGrab(session);
	if(imaqError != IMAQdxErrorSuccess) {
		DriverStation::ReportError("IMAQdxConfigureGrab error: " + std::to_string((long)imaqError) + "\n");
	}


}


void Robot::setLeftSide(double speed) {
	driveFLeft.Set(-speed);
	driveBLeft.Set(-speed);
}

void Robot::setRightSide(double speed) {
	driveFRight.Set(speed);
	driveBRight.Set(speed);
}

START_ROBOT_CLASS(Robot);


